import serial
import time
import os
from pymodbus.client import ModbusSerialClient
import cv2
from ultralytics import YOLO

from pyorbbecsdk import Config
from pyorbbecsdk import OBError
from pyorbbecsdk import OBSensorType, OBFormat
from pyorbbecsdk import Pipeline, FrameSet
from pyorbbecsdk import VideoStreamProfile
from utils import frame_to_bgr_image



global cap, yolo_path, model, ESC_KEY, pipeline, ser, command_get, command_cal


cap = cv2.VideoCapture(0)
cap.set(3, 1280)
yolo_path = "../../yolo/yolov8x-pose.pt"
model = YOLO(yolo_path, task='detect')

ESC_KEY = 27
pipeline = Pipeline()
ser = serial.Serial(
        port = '/dev/ttyUSB0',
        baudrate = 115200,
        timeout = 0.03
    )


command_get = bytes([0x01, 0x03, 0x02, 0x58, 0x00, 0x20, 0xC4, 0x79])

command_cal = [bytes([0x01, 0x10, 0x04, 0x0E, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x14, 0x40, 0xEC]),
               bytes([0x01, 0x10, 0x04, 0x0E, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x15, 0x81, 0x2C]),
               bytes([0x01, 0x10, 0x04, 0x0E, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x16, 0xC1, 0x2D]),
               bytes([0x01, 0x10, 0x04, 0x0E, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x17, 0x00, 0xED]),
               bytes([0x01, 0x10, 0x07, 0xF6, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x14, 0x5A, 0xFE]),
               bytes([0x01, 0x10, 0x07, 0xF6, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x15, 0x9B, 0x3E]),
               bytes([0x01, 0x10, 0x07, 0xF6, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x16, 0xDB, 0x3F]),
               bytes([0x01, 0x10, 0x07, 0xF6, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x17, 0x1A, 0xFF]),
               bytes([0x01, 0x10, 0x0B, 0xDE, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x14, 0x0C, 0x40]),
               bytes([0x01, 0x10, 0x0B, 0xDE, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x15, 0xCD, 0x80]),
               bytes([0x01, 0x10, 0x0B, 0xDE, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x16, 0x8D, 0x81]),
               bytes([0x01, 0x10, 0x0B, 0xDE, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x17, 0x4C, 0x41]),
               bytes([0x01, 0x10, 0x0F, 0xC6, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x14, 0x3E, 0x2A]),
               bytes([0x01, 0x10, 0x0F, 0xC6, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x15, 0xFF, 0xEA]),
               bytes([0x01, 0x10, 0x0F, 0xC6, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x16, 0xBF, 0xEB]),
               bytes([0x01, 0x10, 0x0F, 0xC6, 0x00, 0x02, 0x04, 0x00, 0x00, 0x00, 0x17, 0x7E, 0x2B])]


def plate_calibration(): 
    print("force_plate program start")
    if ser.isOpen():
        print(f'Serial name is {ser.name}')
    else:
        ser.open()
        print("serial is open!")
        print(f'Serial name is {ser.name}')
    for command in command_cal:
        ser.write(command)
        time.sleep(0.1)
        os.system("clear")
        print('calibrating...')



def color_view_config():
    config = Config()

    try:
        profile_list = pipeline.get_stream_profile_list(OBSensorType.COLOR_SENSOR)
        try:
            color_profile: VideoStreamProfile = profile_list.get_video_stream_profile(640, 0, OBFormat.RGB, 30)
        except OBError as e:
            print(e)
            color_profile = profile_list.get_default_video_stream_profile()
            print("color profile: ", color_profile)
        config.enable_stream(color_profile)
    except Exception as e:
        print(e)
        return
    return config




def main():
    plate_calibration()
    config = color_view_config()
    pipeline.start(config)
    while True:
        try:
            ser.write(command_get)
            print('sent, press q to end this program')
            time.sleep(0.02)
            if ser.inWaiting():
                print(ser.inWaiting())
                data = ser.read(ser.inWaiting())
                hex_data = ' '.join(f'{byte:02X}' for byte in data)
                print(hex_data)
            else:
                print('get no data')
            
            frames: FrameSet = pipeline.wait_for_frames(100)
            if frames is None:
                continue
            color_frame = frames.get_color_frame()
            if color_frame is None:
                continue
            # covert to RGB format
            color_image = frame_to_bgr_image(color_frame)
            if color_image is None:
                print("failed to convert frame to image")
                continue
            cv2.imshow("Color Viewer", color_image)
            key = cv2.waitKey(1)
            if key == ord('q') or key == ESC_KEY:
                break
        except KeyboardInterrupt:
            break
    pipeline.stop()
    ser.close()


# def modbus():
#     client = ModbusSerialClient(port = '/dev/ttyUSB0', baudrate = 115200)
#     client.connect()
#     if client.connect():
#         print('client connected')

if __name__ == '__main__':
    main()